package Rangel.policeforce.extaction.clear;

import adf.agent.action.Action;
import adf.agent.info.AgentInfo;
import adf.agent.info.ScenarioInfo;
import adf.agent.info.WorldInfo;
import adf.component.module.algorithm.PathPlanning;
import javolution.util.FastSet;
import Rangel.common.RangelConstants;
import Rangel.common.world.tools.Util;
import Rangel.common.world.helper.RangelWorldHelper;
import Rangel.common.world.entity.RangelBlockade;
import Rangel.common.world.entity.RangelEdge;
import Rangel.common.world.entity.RangelRoad;
import Rangel.common.world.helper.RoadHelper;
import rescuecore2.misc.Pair;
import rescuecore2.standard.entities.*;
import rescuecore2.worldmodel.EntityID;
import java.util.*;


public class ClearBlockadeActExecutor extends ClearActExecutor {

    Set<Blockade> blockadesToClear;
    private Blockade previousBlockade;

    public ClearBlockadeActExecutor(RangelWorldHelper worldHelper, WorldInfo worldInfo, AgentInfo agentInfo, ScenarioInfo scenarioInfo, PathPlanning pathPlanning) {
        super(worldHelper, worldInfo, agentInfo, scenarioInfo, pathPlanning);
    }

    @Override
    public Action clearWay(List<EntityID> pathToGo, EntityID targetID) {
        Blockade targetBlockade = getTargetBlockade(pathToGo, targetID);
        previousBlockade = targetBlockade;
        if (targetBlockade != null) {
            if (clearTools.isInClearRange(targetBlockade)) {
                return sendClearAct(targetBlockade.getID());
            } else {

                if (targetBlockade.getPosition().equals(agentInfo.getPosition())) {
                    return moveToPoint(targetBlockade.getPosition(), targetBlockade.getX(), targetBlockade.getY());
                } else if (pathToGo == null || pathToGo.isEmpty()) {
                    return moveAction((Area) worldInfo.getEntity(targetBlockade.getPosition()));
                } else {
                    return moveAction(pathToGo);

                }

            }
        }

        return null;
    }


    private Blockade getTargetBlockade(List<EntityID> pathToGo, EntityID target) {

        Blockade blockadeToClear = null;
        if (blockadesToClear == null) {
            blockadesToClear = new FastSet<>();
        } else if (!blockadesToClear.isEmpty()) {
            Set<Blockade> tempSet;
            tempSet = refreshFoundBlockades(pathToGo, blockadesToClear);
            blockadesToClear.clear();
            blockadesToClear.addAll(tempSet);
        }


        if (!blockadesToClear.isEmpty()) {
            if (previousBlockade != null) {
                Road road = (Road) worldInfo.getEntity(previousBlockade.getPosition());
                if (!road.isBlockadesDefined() || road.getBlockades().contains(previousBlockade.getID())) {
                    previousBlockade = null;
                } else {
                    blockadesToClear.remove(previousBlockade);
                }
            }
        }


        if (blockadesToClear.isEmpty()) {
            if (pathToGo == null || pathToGo.isEmpty()) {
                if (target != null && agentInfo.getPositionArea() instanceof Road) {
                    blockadesToClear.addAll(clearTools.getTargetRoadBlockades((Road) agentInfo.getPositionArea()));
                }
                if (!blockadesToClear.isEmpty()) {
                    Pair<Blockade, Integer> nearestBlockadePair = clearTools.findNearestBlockade(blockadesToClear);
                    if (nearestBlockadePair != null) {
                        blockadeToClear = nearestBlockadePair.first();
                    }
                }
            } else {
                if (target == null) {
                    blockadesToClear.addAll(findBlockadesInRange(getSelfLocation()));
                } else {
                    blockadesToClear.addAll(findBlockadesOnWay(pathToGo, getRoadsSeen(), target));
                }
                RangelRoad RangelRoad;
                Pair<Integer, Integer> edgeMiddle;
                for (int i = 0; i < pathToGo.size() - 1; i++) {
                    RangelRoad = getRangelRoad(pathToGo.get(i));
                    if (RangelRoad == null || !worldInfo.getChanged().getChangedEntities().contains(pathToGo.get(i))) {
                        continue;
                    }
                    for (RangelEdge RangelEdge : RangelRoad.getRangelEdgesTo(pathToGo.get(i + 1))) {
                        if (RangelEdge.isTooSmall()) {
                            edgeMiddle = new Pair<>((int) RangelEdge.getMiddle().getX(), (int) RangelEdge.getMiddle().getY());
                            Set<Blockade> blockades = clearTools.getBlockadesInRange(edgeMiddle, getBlockadeSeen(), RangelConstants.AGENT_SIZE);
                            blockadesToClear.addAll(blockades);
                        }
                    }
                }

                Pair<Blockade, Integer> nearestBlockadePair = clearTools.findNearestBlockade(blockadesToClear);
                if (nearestBlockadePair != null) {
                    blockadeToClear = nearestBlockadePair.first();
                }

            }

        } else {

            if (pathToGo == null || pathToGo.isEmpty()) {
                blockadesToClear.addAll(findBlockadesInRange(getSelfLocation()));
            } else {
                blockadesToClear.addAll(findBlockadesOnWay(pathToGo, getRoadsSeen(), target));
            }

            Pair<Blockade, Integer> nearestBlockadePair = clearTools.findNearestBlockade(blockadesToClear);

            if (nearestBlockadePair != null) {
                blockadeToClear = nearestBlockadePair.first();
            }
        }

        return blockadeToClear;
    }


    private Set<Blockade> refreshFoundBlockades(List<EntityID> pathToGo, Set<Blockade> blockadesToClear) {

        Set<Blockade> blockadesToRemove = new FastSet<>();
        Set<Blockade> blockades = new FastSet<>(blockadesToClear);
        Road road;
        for (Blockade blockade : blockades) {
            road = (Road) worldInfo.getEntity(blockade.getPosition());
            if (!getRoadsSeen().contains(road) || (road.isBlockadesDefined() && !road.getBlockades().contains(blockade.getID())) || (pathToGo != null && !pathToGo.contains(road.getID()))) {
                blockadesToRemove.add(blockade);
            }
        }

        blockades.removeAll(blockadesToRemove);
        return blockades;

    }

    public Set<Blockade> findBlockadesOnWay(List<EntityID> pathToGo, Set<Road> roadsSeen, EntityID target) {

        Set<RangelBlockade> obstacles_RangelBlockades = new HashSet<>();
        Set<Blockade> blockades = new FastSet<>();
        RangelEdge sourceEdge = null;
        Pair<Integer, Integer> nextMiddlePoint;
        Pair<Integer, Integer> sourceMiddlePoint;

        List<EntityID> seenPath = new ArrayList<>();
        int count = 0;
        for (EntityID entityID : pathToGo) {
            if (worldInfo.getEntity(entityID) instanceof Road) {
                if (!roadsSeen.contains(worldInfo.getEntity(entityID))) {
                    if (pathToGo.size() > count) {
                        seenPath.add(pathToGo.get(count));
                    }
                    break;
                }
            }
            count++;
            seenPath.add(entityID);
        }

        if (seenPath.isEmpty()) {
        } else if (seenPath.size() == 1) {
            if (worldInfo.getEntity(seenPath.get(0)) instanceof Road) {
                if (target != null) {
                    blockades = clearTools.getTargetRoadBlockades((Road) worldInfo.getEntity(seenPath.get(0)));
                } else {
                    blockades = findBlockadesInRange(getSelfLocation());
                }
            }
        } else {
            int j;
            EntityID sourceAreaID;
            EntityID nextAreaID;
            Area sourceArea;
            Area nextArea = null;
            Set<Edge> edgeSet;
            RangelRoad RangelRoad;
            RangelEdge nextEdge = null;
            List<EntityID> neighbours;
            for (int i = 0; i < seenPath.size() - 1; i++) {
                j = i + 1;
                sourceAreaID = seenPath.get(i);
                nextAreaID = seenPath.get(j);
                sourceArea = (Area) worldInfo.getEntity(sourceAreaID);
                nextArea = (Area) worldInfo.getEntity(nextAreaID);
                if (sourceArea instanceof Road) {
                    RangelRoad = getRangelRoad(sourceAreaID);
                    edgeSet = RoadHelper.getEdgesBetween(sourceArea, nextArea);
                    for (Edge edge : edgeSet) {
                        nextEdge = RangelRoad.getRangelEdge(edge);
                        nextMiddlePoint = new Pair<>((int) nextEdge.getMiddle().getX(), (int) nextEdge.getMiddle().getY());
                        if (sourceEdge == null) {

                            if (agentInfo.getPositionArea().equals(sourceArea)) {
                                obstacles_RangelBlockades.addAll(clearTools.getRoadObstacles(RangelRoad, getSelfLocation(), nextMiddlePoint));
                            } else {
                                obstacles_RangelBlockades.addAll(clearTools.getRoadObstacles(RangelRoad, worldInfo.getLocation(sourceArea), nextMiddlePoint));
                            }
                        } else {
                            sourceMiddlePoint = new Pair<>((int) sourceEdge.getMiddle().getX(), (int) sourceEdge.getMiddle().getY());
                            obstacles_RangelBlockades.addAll(clearTools.getRoadObstacles(RangelRoad, sourceMiddlePoint, nextMiddlePoint));
                        }
                    }
                    sourceEdge = nextEdge;


                } else {
                    if (nextArea instanceof Road) {
                        RangelRoad = getRangelRoad(nextAreaID);
                        neighbours = sourceArea.getNeighbours();
                        for (EntityID neighbourID : neighbours) {
                            if (pathToGo.contains(neighbourID) && pathToGo.indexOf(neighbourID) > pathToGo.indexOf(sourceAreaID)) {
                                edgeSet = RoadHelper.getEdgesBetween(sourceArea, nextArea);
                                for (Edge edge : edgeSet) {
                                    nextEdge = RangelRoad.getRangelEdge(edge);
                                    nextMiddlePoint = new Pair<>((int) nextEdge.getMiddle().getX(), (int) nextEdge.getMiddle().getY());
                                    if (sourceEdge == null) {

                                        if (agentInfo.getPositionArea().equals(sourceArea)) {
                                            obstacles_RangelBlockades.addAll(clearTools.getRoadObstacles(RangelRoad, getSelfLocation(), nextMiddlePoint));
                                        } else {
                                            obstacles_RangelBlockades.addAll(clearTools.getRoadObstacles(RangelRoad, worldInfo.getLocation(sourceArea), nextMiddlePoint));
                                        }
                                    } else {
                                        sourceMiddlePoint = new Pair<>((int) sourceEdge.getMiddle().getX(), (int) sourceEdge.getMiddle().getY());
                                        obstacles_RangelBlockades.addAll(clearTools.getRoadObstacles(RangelRoad, sourceMiddlePoint, nextMiddlePoint));
                                    }
                                }
                            }
                        }
                        edgeSet = RoadHelper.getEdgesBetween(sourceArea, nextArea);
                        nextEdge = null;
                        if (edgeSet.iterator().hasNext()) {
                            nextEdge = RangelRoad.getRangelEdge(edgeSet.iterator().next());
                        }
                        sourceEdge = nextEdge;
                    } else {
                        sourceEdge = null;
                    }
                }
            }
            if (nextArea instanceof Road) {
                blockades = clearTools.getTargetRoadBlockades((Road) nextArea);
            }
        }
        for (RangelBlockade blockade : obstacles_RangelBlockades) {
            blockades.add(blockade.getParent());
        }


        return blockades;

    }

    private Set<Blockade> findBlockadesInRange(Pair<Integer, Integer> position) {
        Set<Blockade> blockadeSet = new FastSet<>();
        for (Blockade blockade : getBlockadeSeen()) {

            if (blockade.getShape().contains(position.first(), position.second()) || Util.findDistanceTo(blockade, position.first(), position.second()) < 1000) {
                blockadeSet.add(blockade);
            }
        }
        return blockadeSet;
    }

}
